#include "wearhouse_navigation/navigation_manager.hpp"
#include <tf2/LinearMath/Quaternion.h> // 用于创建四元数
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <iostream>
#include <yaml-cpp/yaml.h> // 如果你想从YAML文件加载
#include <fstream>
#include <filesystem> // C++17, 用于检查文件是否存在

namespace wearhouse_navigation {

    NavigationManager::NavigationManager()
        : Node("navigation_manager"),
          current_waypoint_index_(0),
          is_executing_plan_(false)
    {
        // 1. 创建 Action Client，连接到 WearhouseNavigator 的 Action Server
        action_client_ = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");

        // 2. 创建 Subscriber，接收目标井编号
        target_well_sub_ = this->create_subscription<std_msgs::msg::String>(
            "target_well", 10, // 话题名称和队列大小
            std::bind(&NavigationManager::targetWellCallback, this, std::placeholders::_1)
        );

        // 3. 加载井的路径映射
        loadWellPathMap();

        RCLCPP_INFO(this->get_logger(), "Navigation Manager initialized.");
    }

    void NavigationManager::loadWellPathMap() {
        // --- 方法一：从 YAML 文件加载 ---
        // 你可以创建一个 YAML 文件，例如 config/well_paths.yaml
        std::string yaml_file_path = this->get_parameter("config_file_path").as_string_or("config/well_paths.yaml");
        if (std::filesystem::exists(yaml_file_path)) {
            try {
                YAML::Node config = YAML::LoadFile(yaml_file_path);
                for (const auto& well_node : config["wells"]) {
                    std::string well_id = well_node["id"].as<std::string>();
                    std::vector<Waypoint> path;
                    for (const auto& wp_node : well_node["path"]) {
                        Waypoint wp;
                        wp.x = wp_node["x"].as<double>();
                        wp.y = wp_node["y"].as<double>();
                        wp.theta = wp_node["theta"].as<double>(); // 可选
                        wp.description = wp_node["desc"].as<std::string>(); // 可选
                        path.push_back(wp);
                    }
                    well_path_map_[well_id] = path;
                    RCLCPP_INFO(this->get_logger(), "Loaded path for well '%s' with %zu waypoints.", well_id.c_str(), path.size());
                }
            } catch (const YAML::Exception& e) {
                RCLCPP_WARN(this->get_logger(), "Failed to load YAML config: %s. Using default/empty map.", e.what());
            }
        } else {
            RCLCPP_WARN(this->get_logger(), "Config file '%s' does not exist. Using default/empty map.", yaml_file_path.c_str());
        }

        // --- 方法二：硬编码（作为示例或备用） ---
        // 如果文件加载失败或不存在，可以使用硬编码数据
        if (well_path_map_.empty()) {
            RCLCPP_WARN(this->get_logger(), "Using hardcoded well paths as fallback.");
            // 示例：为井 A1, A2, B1 定义路径
            {
                std::vector<Waypoint> path_a1;
                Waypoint wp1_a1, wp2_a1, wp3_a1;
                wp1_a1.x = 0.0; wp1_a1.y = 0.0; wp1_a1.theta = 0.0; wp1_a1.description = "Start";
                wp2_a1.x = 1.0; wp2_a1.y = 0.0; wp2_a1.theta = 0.0; wp2_a1.description = "Intermediate";
                wp3_a1.x = 1.0; wp3_a1.y = 1.0; wp3_a1.theta = M_PI / 2; wp3_a1.description = "Final";
                path_a1.push_back(wp1_a1);
                path_a1.push_back(wp2_a1);
                path_a1.push_back(wp3_a1);
                well_path_map_["A1"] = path_a1;
            }
            {
                std::vector<Waypoint> path_a2;
                Waypoint wp1_a2, wp2_a2, wp3_a2;
                wp1_a2.x = 0.0; wp1_a2.y = 0.0; wp1_a2.theta = 0.0; wp1_a2.description = "Start";
                wp2_a2.x = 1.5; wp2_a2.y = 0.0; wp2_a2.theta = 0.0; wp2_a2.description = "Intermediate";
                wp3_a2.x = 1.5; wp3_a2.y = 1.5; wp3_a2.theta = M_PI / 2; wp3_a2.description = "Final";
                path_a2.push_back(wp1_a2);
                path_a2.push_back(wp2_a2);
                path_a2.push_back(wp3_a2);
                well_path_map_["A2"] = path_a2;
            }
            {
                std::vector<Waypoint> path_b1;
                Waypoint wp1_b1, wp2_b1, wp3_b1;
                wp1_b1.x = 0.0; wp1_b1.y = 0.0; wp1_b1.theta = 0.0; wp1_b1.description = "Start";
                wp2_b1.x = 0.0; wp2_b1.y = 1.0; wp2_b1.theta = M_PI / 2; wp2_b1.description = "Intermediate";
                wp3_b1.x = 2.0; wp3_b1.y = 1.0; wp3_b1.theta = 0.0; wp3_b1.description = "Final";
                path_b1.push_back(wp1_b1);
                path_b1.push_back(wp2_b1);
                path_b1.push_back(wp3_b1);
                well_path_map_["B1"] = path_b1;
            }
        }
    }

    void NavigationManager::targetWellCallback(const std_msgs::msg::String::SharedPtr msg) {
        std::string target_well_id = msg->data;
        RCLCPP_INFO(this->get_logger(), "Received request to navigate to well: '%s'", target_well_id.c_str());

        if (is_executing_plan_) {
            RCLCPP_WARN(this->get_logger(), "A navigation plan is already executing. Ignoring new request for well '%s'.", target_well_id.c_str());
            return;
        }

        if (well_path_map_.find(target_well_id) == well_path_map_.end()) {
            RCLCPP_ERROR(this->get_logger(), "Well ID '%s' not found in path map. Cannot start navigation.", target_well_id.c_str());
            return;
        }

        startNavigationPlan(target_well_id);
    }

    void NavigationManager::startNavigationPlan(const std::string& well_id) {
        current_path_ = well_path_map_[well_id];
        if (current_path_.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Path for well '%s' is empty. Cannot start plan.", well_id.c_str());
            return;
        }
        is_executing_plan_ = true;
        current_waypoint_index_ = 0;
        RCLCPP_INFO(this->get_logger(), "Starting navigation plan for well '%s' with %zu waypoints.", well_id.c_str(), current_path_.size());
        sendNextWaypoint();
    }

    void NavigationManager::sendNextWaypoint() {
        if (current_waypoint_index_ >= current_path_.size()) {
            RCLCPP_INFO(this->get_logger(), "Navigation plan for well completed successfully!");
            is_executing_plan_ = false;
            current_path_.clear(); // 清理当前路径
            return;
        }

        if (!action_client_->wait_for_action_server(std::chrono::seconds(5))) {
            RCLCPP_ERROR(this->get_logger(), "Action server 'navigate_to_pose' not available after waiting");
            is_executing_plan_ = false;
            current_path_.clear();
            return;
        }

        auto goal_msg = NavigateToPose::Goal();
        // 构造 geometry_msgs::msg::PoseStamped
        goal_msg.target_pose.header.frame_id = "map"; // 假设所有坐标都在 map 坐标系
        goal_msg.target_pose.header.stamp = this->get_clock()->now();
        goal_msg.target_pose.pose.position.x = current_path_[current_waypoint_index_].x;
        goal_msg.target_pose.pose.position.y = current_path_[current_waypoint_index_].y;
        goal_msg.target_pose.pose.position.z = 0.0; // 2D导航，z为0

        tf2::Quaternion q;
        q.setRPY(0, 0, current_path_[current_waypoint_index_].theta); // RPY都为0，只有偏航角
        goal_msg.target_pose.pose.orientation = tf2::toMsg(q);

        RCLCPP_INFO(this->get_logger(), "[Plan for Well] Sending waypoint %zu (%s): x=%.2f, y=%.2f, theta=%.2f",
                    current_waypoint_index_ + 1, current_path_[current_waypoint_index_].description.c_str(),
                    goal_msg.target_pose.pose.position.x,
                    goal_msg.target_pose.pose.position.y,
                    current_path_[current_waypoint_index_].theta);

        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        send_goal_options.goal_response_callback = std::bind(&NavigationManager::goalResponseCallback, this, std::placeholders::_1);
        send_goal_options.feedback_callback = std::bind(&NavigationManager::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback = std::bind(&NavigationManager::resultCallback, this, std::placeholders::_1);

        action_client_->async_send_goal(goal_msg, send_goal_options);
    }

    void NavigationManager::goalResponseCallback(std::shared_future<GoalHandle::SharedPtr> future) {
        auto goal_handle = future.get();
        if (!goal_handle) {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        } else {
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
    }

    void NavigationManager::feedbackCallback(
        GoalHandle::SharedPtr goal_handle,
        const std::shared_ptr<const NavigateToPose::Feedback> feedback)
    {
        (void)goal_handle; // Not used here
        RCLCPP_INFO(this->get_logger(), "[Waypoint %zu] Feedback: %s, Distance remaining: %.2f",
                    current_waypoint_index_ + 1, feedback->status.c_str(), feedback->distance_remaining);
    }

    void NavigationManager::resultCallback(const GoalHandle::WrappedResult & result) {
        switch (result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "[Waypoint %zu] SUCCEEDED. Moving to next waypoint.", current_waypoint_index_ + 1);
                current_waypoint_index_++;
                sendNextWaypoint(); // 发送下一个路标点
                break;
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(this->get_logger(), "[Waypoint %zu] was ABORTED. Stopping plan.", current_waypoint_index_ + 1);
                is_executing_plan_ = false;
                current_path_.clear();
                break;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_WARN(this->get_logger(), "[Waypoint %zu] was CANCELED. Stopping plan.", current_waypoint_index_ + 1);
                is_executing_plan_ = false;
                current_path_.clear();
                break;
            default:
                RCLCPP_ERROR(this->get_logger(), "[Waypoint %zu] Unknown result code. Stopping plan.", current_waypoint_index_ + 1);
                is_executing_plan_ = false;
                current_path_.clear();
                break;
        }
    }

} // namespace wearhouse_navigation

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(wearhouse_navigation::NavigationManager)